package bluetooth;
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;

import Robot.SensorColour;


import lejos.pc.comm.NXTComm;
import lejos.pc.comm.NXTCommException;
import lejos.pc.comm.NXTCommFactory;
import lejos.pc.comm.NXTInfo;
import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.Pose;


public class ProtocolPilot implements IndigoProtocol 
{
	
	private DataOutputStream dataOut;
	private DataInputStream dataIn;
	
	public ProtocolPilot()
	{
		try {
			connect("indigo", "00:16:53:0a:0d:24");
		} catch (NXTCommException e) {
			System.out.println("Failed to connect to brick, abort mission.");
		}
	}
	
	private void connect(String brickName, String brickAdress) throws NXTCommException
	{
		System.out.println("Connecting to brick " + brickName + " on adress " + brickAdress);
		NXTInfo nxtInfo = new NXTInfo(NXTCommFactory.BLUETOOTH, brickName, brickAdress);
		NXTComm nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
		nxtComm.open(nxtInfo);
		dataOut = new DataOutputStream(nxtComm.getOutputStream());
		dataIn  = new DataInputStream(nxtComm.getInputStream());
		System.out.println("Connected");
	}
	
	public synchronized boolean isMoving()
	{
		sendData(ISMOVING, 0, 0, true);
		try {
			return dataIn.readBoolean();
		} catch (IOException e) {
			return false;
		}
	}
	
	public synchronized int getSonarTachoCount()
	{
		sendData(GETSONARTACHO, 0, 0, true);
		try {
			return dataIn.readInt();
		} catch (IOException e) {
			return -1;
		}
	}
	
	public synchronized float getMaxRotateSpeed()
	{
		sendData(GETMAXROTATESPEED, 0, 0, true);
		try {
			return dataIn.readFloat();
		} catch (IOException e) {
			return -1;
		}
	}
	
	public synchronized Move getMovement()
	{
		sendData(GETMOVEMENT, 0, 0, true);
		Move move = new Move(1, 1, true);
		try {
			move.loadObject(dataIn);
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		return move;
	}
	
	public synchronized double getMaxTravelSpeed()
	{
		sendData(GETMAXTRAVELSPEED, 0, 0, true);
		try {
			return dataIn.readDouble();
		} catch (IOException e) {
			return -1;
		}
	}
	
	public synchronized void forward()
	{
		sendData(FORWARD, 0, 0, true);
	}
	
	public synchronized void backward()
	{
		sendData(BACKWARD, 0, 0, true);
	}
	
	public synchronized void stop()
	{
		sendData(STOP, 0, 0, true);
	}
	
	public synchronized void travel(double distance, boolean immediateReturn)
	{
		sendData(TRAVEL, distance, 0, immediateReturn);
	}
	
	public synchronized void travel(double distance)
	{
		this.travel(distance, false);
	}
	
	/**
	 * 
	 * @param distance
	 * 			de afstand om te rijden.
	 * @return	Een getal dat het resultaat van het rijden teruggeeft.
	 * 			Als het getal anders is dan 0 is de robot gestopt.
	 * 			0: geen probleem
	 * 			1: zwart gevonden
	 * 			2: gebotst
	 * 			3: sonarafstand kleiner dan 10
	 */
	public synchronized int travelSafely(double distance)
	{
		sendData(TRAVELSAFELY, distance, 0, true);
		
		try {
			return dataIn.readInt();
		} catch (IOException e) {
			return -1;
		}
	}
	
	public synchronized void rotate(double angle, boolean immediateReturn)
	{
		sendData(ROTATE, angle, 0, immediateReturn);
	}
	
	public synchronized void rotate(double angle)
	{
		this.rotate(angle, false);
	}
	
	public synchronized void rotateSonar(double angle)
	{
		sendData(ROTATE_SONAR, angle, 0, false);
	}
	
	public synchronized void setTravelSpeed(double value)
	{
		sendData(SETTRAVELSPEED, value, 0, true);
	}
	
	public synchronized void setRotateSpeed(double value)
	{
		sendData(SETROTATESPEED, value, 0, true);
	}
	
	public synchronized void setAcceleration(int value)
	{
		
		sendData(SETACCELERATION, (double) value, 0, true);
	}
	
	public synchronized int getLightValue()
	{
		sendData(GETLIGHTVALUE, 0, 0, true);
		try {
			return dataIn.readInt();
		} catch (IOException e) {
			return -1;
		}
	}
	
	public synchronized float getSonarDistance() 
	{
	
		sendData(GETSONARVALUE, 0, 0, true);
		try {
			return dataIn.readFloat();
		} catch (IOException e) {
			return -1;
		}
	}
	  
	public synchronized boolean isTouchLeftPressed()
	{
		sendData(GETTOUCHLEFT, 0, 0, true);
		try {
			return dataIn.readBoolean();
		} catch (IOException e) {
			e.printStackTrace();
			return false;
		}
	}
	
	public synchronized boolean isTouchRightPressed()
	{
		sendData(GETTOUCHRIGHT, 0, 0, true);
		try {
			return dataIn.readBoolean();
		} catch (IOException e) {
			return false;
		}
	}
	
	public synchronized void rotateLeft()
	{
		sendData(ROTATELEFT, 0, 0, true);
	}

	public synchronized void rotateRight()
	{
		sendData(ROTATERIGHT, 0, 0, true);
	}
	
	public synchronized Pose getPose()
	{
		sendData(GETPOSE, 0, 0, true);
		Pose pose = new Pose();
		try {
			pose.loadObject(dataIn);
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		return pose;
	}
	
	public synchronized void setPose(Pose pose)
	{
		sendData(SETPOSE, 0, 0, true);
		try {
			pose.dumpObject(dataOut);
		} catch (Exception e) {
			System.out.println("Failed sending pose from pc to robot");
		}
	}
	
	public synchronized void findWhiteLine()
	{
		System.out.println("Sending white line to robot");
		sendData(FINDWHITELINE, 0, 0, false);	
		System.out.println("Line done");
	}
	
	public synchronized int scanBarcode()
	{
		sendData(SCANBARCODE, 0, 0, true);
		System.out.println("Waiting for data");
		try {
			
			int result = dataIn.readInt();
			if(!simulator.labyrinth.BarCode.isValidCode(result))
				result = simulator.labyrinth.BarCode.calculateInverseNumber(result);
			System.out.println("Data received: " + result);
			return result;
		} catch (IOException e) {
			return -1;
		}
	}
	public synchronized void goToMiddle()
	{	System.out.println("Sending gotomiddle to robot");
		sendData(GOTOMIDDLE, 0, 0, false);
		System.out.println("in midden");
	}
	public synchronized float getSafeSonarDistance() 
	{
	
		sendData(SCANSAFE, 0, 0, true);
		try {
			return dataIn.readFloat();
		} catch (IOException e) {
			return -1;
		}
	}
	public synchronized void setStraight()
	{	System.out.println("Sending setStraight to robot");
		sendData(SETSTRAIGHT, 0, 0, false);
		System.out.println("in midden");
	}
	/**
	 * 
	 * @param direction
	 * 			de richting om te scannen:
	 * 		   -1 : links
	 * 			0 : vooruit
	 * 			1 : rechts
	 * @return	Mediaan van de scan
	 */
	public synchronized int sonarScan(int direction)
	{
		sendData(SONARSCAN, direction, 0, true);
		try {
			return dataIn.readInt();
		} catch (IOException e) {
			return -1;
		}	
	}
	
	/**
	 * 
	 * @param color De kleur om te calibreren
	 * 			
	 * @return
	 */
	public synchronized int calibrateColor(SensorColour color)
	{
		if(color == SensorColour.WHITE)
			sendData(CALIBRATE_COLOR, 1, 0, true);
		else if(color == SensorColour.BLACK)
			sendData(CALIBRATE_COLOR, 1, 0, true);
		else
			sendData(CALIBRATE_COLOR, 3, 0, true);
		
		try {
			return dataIn.readInt();
		} catch (IOException e) {
			return -1;
		}	
	}
	
	public synchronized void playTune(boolean immediateReturn) {
		sendData(PLAY_TUNE, 0, 0, immediateReturn);
	}
	
	private void sendData(int OPCode, double param1, double param2, boolean immediateReturn)
	{
		try 
		{
			dataOut.writeInt(OPCode);
			dataOut.writeDouble(param1);
			dataOut.writeDouble(param2);
			dataOut.writeBoolean(immediateReturn);
			dataOut.flush();
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}

		
		if(!immediateReturn) // Wait for robot reply
		{
			try {
				dataIn.readBoolean();
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
	}
}
